#include "spacecol.h"
#include "spacecol2d.h"

#define BPR 7
#define ROH2D 1
#define R2D 6

SpaceCol::SpaceCol()
{
    numBlock = 0;
    numBuds = 0;
    ROH = ROH2D;
    R = R2D;
}

SpaceCol::~SpaceCol()
{
     if (m_tree) m_tree->freeTree(m_tree->treeRoot);
     delete[] m_blocks;
     m_blocks = NULL;

}


void SpaceCol::populateBudD(Bud * b, int depth)
{

    if (!b->aux)
    {
        int l = b->assocMarkers.size();
        b->aQ = 0;
        float3 V = float3(0,0,0);
        for (int i = 0; i < l; i++)
        {
            double * currentM = b->assocMarkers.at(i);
            if ((int) *(currentM + 3) == (int) b->spaceColIndex)
            {
                // if this markers belong to the perception volume of this bud
                b->aQ = 1;
                // calculate optimal growth direction.
                float3 v = float3(*(currentM + 0), *(currentM + 1), *(currentM + 2)) - b->end;
                v.normalize();
                V += v;

            }
        }
        if (V != float3(0,0,0))
            V.normalize();
        b->aV = V;
         //cout << "populate bud aux" << b->aV << "--" << b->aQ << endl;
    }
    if (!b->term)
    {
        // calculate what block is b->end in
        int l = b->assocMarkers.size();
        b->tQ = 0;
        float3 V = float3(0,0,0);
        for (int i = 0; i < l; i++)
        {
            double * currentM = b->assocMarkers.at(i);
            if ((int) *(currentM + 3) == (int) b->spaceColIndex)
            {
                // if this markers belong to the perception volume of this bud
                b->tQ = 1;
                // calculate optimal growth direction.
                float3 v = float3(*(currentM + 0), *(currentM + 1), *(currentM + 2)) - b->end;
                v.normalize();
                V += v;
            }
        }
        if (V != float3(0,0,0))
            V.normalize();
        b->tV = V;
        // cout << "populate bud term:" << b->tV << b->tQ << endl;
    }
    if (b->aux)
            populateBudD(b->aux,depth);
    if (b->term)
        populateBudD(b->term,depth);

}

void SpaceCol::cleanDeletedMarkers()
{
    for (int i = 0; i < numBlock; i++)
    {
        Block * currentBlock = &(m_blocks[i]);
        for (int j = 0; j < currentBlock->m_markers.size()/5.0; j++)
        {
            if (currentBlock->m_markers[j*5 + 3] < -1.9 && currentBlock->m_markers[j*5 + 4] < -1.9)
            {
                currentBlock->m_markers.erase(currentBlock->m_markers.begin() + j*5 + 4);
                currentBlock->m_markers.erase(currentBlock->m_markers.begin() + j*5 + 3);
                currentBlock->m_markers.erase(currentBlock->m_markers.begin() + j*5 + 2);
                currentBlock->m_markers.erase(currentBlock->m_markers.begin() + j*5 + 1);
                currentBlock->m_markers.erase(currentBlock->m_markers.begin() + j*5 + 0);
            }
        }
    }
}

void SpaceCol::assignMarkersD(Bud *b)
{

    if (b->assocMarkers.size() > 0)
        b->assocMarkers.clear();

    cleanDeletedMarkers();

    // TERMINAL BUDS:
    if ( !b->term || !b->aux )
    {
       // cout << "herehere" << endl;
        int count;
        // calculate what block is b->end in
            double size = blockSize;
            int col = (int)b->end.x/size;
            int row = (int)b->end.y/size;
            int depth = (int) b->end.z/size;
            // loop through the blocks, get 9 surrounding blocks
            count = 0;
            for (int i = 0; i < numBlock; i++)
            {
                int cdepth = i / BPRSQ;
                int index2d = i % BPRSQ;
                int ccol = index2d % BPR;
                int crow = index2d / BPR;

                if ((ccol <= col + 1 && ccol >= col - 1) && (crow <= row + 1 && crow >= row - 1) && (cdepth <= depth + 1 && cdepth >= depth - 1) )
                {

                        count ++;
                        Block * currentBlock = &(m_blocks[i]);
                        for (int j = 0; j < currentBlock->m_markers.size()/5.0; j++)
                        {
                            ///if (currentBlock->m_markers[j*5 + 3] > -1.9 && currentBlock->m_markers[j*5 + 4] > -1.9)
                            {

                                // delete markers in occupancy zone
                                float x = currentBlock->m_markers[j*5 + 0];
                                float y = currentBlock->m_markers[j*5 + 1];
                                float z = currentBlock->m_markers[j*5 + 2];
                                float3 m = float3(x,y,z);
                                float distance = m.getDistance(b->end);

                                if (distance <= b->length*ROH)
                                {
                                   // delete markers
                                    currentBlock->m_markers[j*5 + 3] = -2;
                                    currentBlock->m_markers[j*5 + 4] = -2;
                                }
                                else if (distance > b->length*ROH && distance <= b->length*R)
                                {
                                    float3 v = m - b->end;
                                    v.normalize();
                                    float angle = v.dot(b->direction);
                                    if ( (angle) >= (cos(THETAHALF)))
                                    {
                                        // within perception volume:
                                        if ((currentBlock->m_markers[j*5 + 4] > 0 &&currentBlock->m_markers[j*5 + 4] > distance) || (currentBlock->m_markers[j*5 + 4] < 0 && currentBlock->m_markers[j*5 + 4] > -1.9))
                                        {
                                            // if the marker is not deleted and distance is smaller than current distance in the markers
                                            //cout << "assign markers" << endl;
                                            currentBlock->m_markers[j*5 + 3] = b->spaceColIndex;
                                            currentBlock->m_markers[j*5 + 4] = distance;
                                            b->assocMarkers.push_back(&(currentBlock->m_markers[j*5 + 0]));
                                        }
                                    }
                                }
                            }
                        }
                }
            }

    }

    if (b->term)
        assignMarkersD(b->term);
    if (b->aux)
       assignMarkersD(b->aux);

}


void SpaceCol::assignMarkersB(Bud *b)
{

    if (b->auxMarkers.size() > 0)
       b->auxMarkers.clear();
    if (b->termMarkers.size() > 0)
        b->termMarkers.clear();

    cleanDeletedMarkers();

    // AUXILIARY BUD:
    if (!b->aux )
    {

        int count;
        // calculate what block is b->end in
            double size = blockSize;
            int col = (int)b->end.x/size;
            int row = (int)b->end.y/size;
            int depth = (int) b->end.z/size;
            // loop through the blocks, get 9 surrounding blocks
            count = 0;
            for (int i = 0; i < numBlock; i++)
            {
                int cdepth = i / BPRSQ;
                int index2d = i % BPRSQ;
                int ccol = index2d % BPR;
                int crow = index2d / BPR;
                if ((ccol <= col + 1 && ccol >= col - 1) && (crow <= row + 1 && crow >= row - 1)  && (cdepth <= depth + 1 && cdepth >= depth - 1) )
                {
                        count ++;

                        Block * currentBlock = &(m_blocks[i]);
                        for (int j = 0; j < currentBlock->m_markers.size()/5.0; j++)
                        {
                           // if (currentBlock->m_markers[j*5 + 3] > -1.9 && currentBlock->m_markers[j*5 + 4] > -1.9)
                            {
                                float x = currentBlock->m_markers[j*5 + 0];
                                float y = currentBlock->m_markers[j*5 + 1];
                                float z = currentBlock->m_markers[j*5 + 2];
                                float3 m = float3(x,y,z);
                                float distance = m.getDistance(b->end);

                                if (distance <= b->length*ROH)
                                {
                                   // delete markers
                                    currentBlock->m_markers[j*5 + 3] = -2;
                                    currentBlock->m_markers[j*5 + 4] = -2;
                                }
                                else if (distance > b->length*ROH && distance <= b->length*R)
                                {

                                    float3 v = m - b->end;
                                    v.normalize();
                                    b->auxDir.normalize();
                                    //cout << "auxDir: " << b->auxDir << endl;
                                    float angle = v.dot(b->auxDir);
                                    if ( (angle) >= (cos(THETAHALF)))
                                    {
                                        // within perception volume:
                                        if ((currentBlock->m_markers[j*5 + 4] > 0 && currentBlock->m_markers[j*5 + 4] > distance) || (currentBlock->m_markers[j*5 + 4] < 0 && currentBlock->m_markers[j*5 + 4] > -1.9))
                                        {
                                            // if the marker is not deleted and distance is smaller than current distance in the markers
                                           // cout << "assign markers" << endl;
                                            currentBlock->m_markers[j*5 + 3] = b->spaceColIndex;
                                            currentBlock->m_markers[j*5 + 4] = distance;
                                            b->auxMarkers.push_back(&(currentBlock->m_markers[j*5 + 0]));
                                        }
                                    }
                                }
                            }
                        }
                }
            }

    }

    // TERMINAL BUDS:
    if ( !b->term )
    {
        int count;
        // calculate what block is b->end in
            double size = blockSize;
            int col = (int)b->end.x/size;
            int row = (int)b->end.y/size;
            int depth = (int) b->end.z/size;
            // loop through the blocks, get 9 surrounding blocks
            count = 0;
            for (int i = 0; i < numBlock; i++)
            {
                int cdepth = i / BPRSQ;
                int index2d = i % BPRSQ;
                int ccol = index2d % BPR;
                int crow = index2d / BPR;

                if ((ccol <= col + 1 && ccol >= col - 1) && (crow <= row + 1 && crow >= row - 1) && (cdepth <= depth + 1 && cdepth >= depth - 1) )
                {

                        count ++;
                        Block * currentBlock = &(m_blocks[i]);
                        for (int j = 0; j < currentBlock->m_markers.size()/5.0; j++)
                        {
                            ///if (currentBlock->m_markers[j*5 + 3] > -1.9 && currentBlock->m_markers[j*5 + 4] > -1.9)
                            {

                                // delete markers in occupancy zone
                                float x = currentBlock->m_markers[j*5 + 0];
                                float y = currentBlock->m_markers[j*5 + 1];
                                float z = currentBlock->m_markers[j*5 + 2];
                                float3 m = float3(x,y,z);
                                float distance = m.getDistance(b->end);

                                if (distance <= b->length*ROH)
                                {
                                   // delete markers
                                    currentBlock->m_markers[j*5 + 3] = -2;
                                    currentBlock->m_markers[j*5 + 4] = -2;
                                }
                                else if (distance > b->length*ROH && distance <= b->length*R)
                                {
                                    float3 v = m - b->end;
                                    v.normalize();
                                    float angle = v.dot(b->direction);
                                    if ( (angle) >= (cos(THETAHALF)))
                                    {
                                        // within perception volume:
                                        if ((currentBlock->m_markers[j*5 + 4] > 0 &&currentBlock->m_markers[j*5 + 4] > distance) || (currentBlock->m_markers[j*5 + 4] < 0 && currentBlock->m_markers[j*5 + 4] > -1.9))
                                        {
                                            // if the marker is not deleted and distance is smaller than current distance in the markers
                                           // cout << "assign markers" << endl;
                                            currentBlock->m_markers[j*5 + 3] = b->spaceColIndex;
                                            currentBlock->m_markers[j*5 + 4] = distance;
                                            b->termMarkers.push_back(&(currentBlock->m_markers[j*5 + 0]));
                                        }
                                    }
                                }
                            }
                        }
                }
            }

    }

    if (b->term)
        assignMarkersB(b->term);
    if (b->aux)
       assignMarkersB(b->aux);

}

void SpaceCol::populateBudB(Bud * b, int depth)
{

    if (!b->aux)
    {
        int l = b->auxMarkers.size();
        b->aQ = 0;
        float3 V = float3(0,0,0);
        for (int i = 0; i < l; i++)
        {
            double * currentM = b->auxMarkers.at(i);
            if ((int) *(currentM + 3) == (int) b->spaceColIndex)
            {
                // if this markers belong to the perception volume of this bud
                b->aQ = 1;
                // calculate optimal growth direction.
                float3 v = float3(*(currentM + 0), *(currentM + 1), *(currentM + 2)) - b->end;
                v.normalize();
                V += v;


            }
        }
        if (V != float3(0,0,0))
            V.normalize();
        b->aV = V;
        // cout << "populate bud aux" << b->aV << "--" << b->aQ << endl;
    }
    if (!b->term)
    {
        // calculate what block is b->end in
        int l = b->termMarkers.size();
        b->tQ = 0;
        float3 V = float3(0,0,0);
        for (int i = 0; i < l; i++)
        {
            double * currentM = b->termMarkers.at(i);
            if ((int) *(currentM + 3) == (int) b->spaceColIndex)
            {
                // if this markers belong to the perception volume of this bud
                b->tQ = 1;
                // calculate optimal growth direction.
                float3 v = float3(*(currentM + 0), *(currentM + 1), *(currentM + 2)) - b->end;
                v.normalize();
                V += v;
            }
        }
        if (V != float3(0,0,0))
            V.normalize();
        b->tV = V;
        // cout << "populate bud term:" << b->tV << b->tQ << endl;
    }
    if (b->aux)
            populateBudB(b->aux,depth);
    if (b->term)
        populateBudB(b->term,depth);

}
